/**
 * Implement the aabbtree structure. 
 * @author Songgang Xu (sxu at tamu dot edu)
 * @file aabbtree.h
 */

#ifndef _AABBTREE_H
#define _AABBTREE_H

#include <vector>
#include <cstdio>
#include <cstring>


const float FLOAT_INFINITY = 1e5 ;

/**
 * Defines a polygon class used by AABBTree. It is an inherited class from Polygon. 
 * We use vertex idx to contruct polygon, which reduce the memory cost. 
 */
template <typename Value>
class AABBTreePolygon  : public Polygon <Value>
{
protected :
	typedef Value ValueType ; 
	typedef typename Vector <Value>::PointType PointType ;
	typedef Vector <Value>            VectorType ; 
	typedef Plane <Value>             PlaneType ;
	typedef typename MeshPrimitive<Value>::FaceType     FaceType ;
public:
    
    /**
     * Creates a new empty polygon.
     */
    AABBTreePolygon() 
		:face_idx(-1), face(NULL)
	{
		nbIndices_ = 0;
	}

    /**
     * Destructs this polygon instance.
     */
    ~AABBTreePolygon() 
	{
	
	}

    /**
     * Tells if this polygon is degenerated.
     * A degenerated polygon is a polygon where two or more points are the same.
     * @return true if the polygon is degenerated, false otherwhise.
     */
    bool is_degenerate() const {return (nbIndices_ < 3); }

    /**
     * Copies this polygon (nbIndices, indices and normal) to one another polygon.
     * It causes its content to be erased by this one.
     * @param polygon The Polygon where the copy will occur.
     */
    void copyTo(AABBTreePolygon & _pol) const 
	{
		_pol.~Polygon();
		_pol.nbIndices_ = nbIndices_;
		_pol.normal_ = this->m_normal;
		_pol.m_indices = this->m_indices ;
	}

    /**
     * Clones the polygon.
     * @return A pointer to a cloned polygon.
     */
    AABBTreePolygon * clone() const 
	{
		AABBTreePolygon *pol = new AABBTreePolygon();
		copyTo(*pol);
		return pol;		
	}

    /**
     * Set the number indices of this polygon.
     * Note that changing the number of indices doesn't erase the old content of the object.
     * For example passing from a triangle (3 indices) to a quadrangle (4 indices) keeps
     * the 3 first indices intact.
     * This methods make uses of malloc() and so, is slower than setIndicesMemory()
     */
    void set_nb_vertices(unsigned _nbIndices)
	{
		this->nbIndices_ = _nbIndices ;
		this->m_indices.resize (this->nbIndices_);
	}
	

    /**
     * Gets the number of indices used by this polygon.
     * A polygon of 1 index is a point, two indices is a line, three indices is a triangle, etc.
     * Maximum number of indices is 255.
     */
    unsigned get_nb_vertices() const { return nbIndices_ & 0xff; }

    /**
     * Gets a point index for this polygon.
     * @return The point index.
     */
    int getIndex(int i) const { return m_indices[i]; }
    
    /**
     * Sets a point index for this polygon.
     * @param i The index inside the polygon.
     * @param index The point index.
     */
    void setIndex(int i, int index) { m_indices[i] = index; }

	/**
	 * Convert a AABBTreePolygon to a simple polygon. 
	 */
	void simplePolygon (Polygon<ValueType> & _polygon, std::vector<PointType> points) const 
	{
		for (int i = 0; i < nbIndices_ ; ++i)
		{
			_polygon.append_vertex (points [m_indices[i]]);
		}
	}

	/**
	 * Set the corresponding face idx. 
	 * @pre{ The polygon is created from a mesh face.}
	 */
	void set_face_idx (int _idx) {this->face_idx  = _idx; }
	
	/**
	 * Get the corresponding face idx. 
	 * @pre{ The polygon is created from a mesh face.}
	 */
	int get_face_idx () const { return this->face_idx;  }

	/**
	 * Set the corresponding face pointer.
	 * @pre{ The polygon is created from a mesh face.}
	 */
	void set_face (FaceType * _face) {this->face = _face ;}

	/**
	 * Get the pointer of the face. 
	 * @pre{ The polygon is created from a mesh face.}
	 */
	FaceType * get_face() const  {return this->face; }
private:
	int          face_idx ;
    int          nbIndices_;
	std::vector<int> m_indices; 
	FaceType *  face ;
};



/**
 * Define the AABBTreeNode class. 
 * The node could be polygon, sphere or aabbtree. 
 */
template <typename Value> 
class AABBTreeNode
{
protected:
	typedef Value ValueType ; 
	typedef typename Vector <Value>::PointType PointType ;
	typedef Vector <Value>            VectorType ; 
	typedef Box <Value>               BoxType ;
public:
	BoxType aabb ;
	const AABBTreeNode * left ;
	const AABBTreeNode * right ;
private:
};

/**
 * Define the node type of AABBTree. 
 */
enum AABBTREE_LEAFTYPE
{
	AABBTREE_LEAFTYPE_POLYGON,
	AABBTREE_LEAFTYPE_SPHERE,
	AABBTREE_LEAFTYPE_AABB 
};

/**
 * AABBTree node for polygon. 
 */
template <typename Value> 
class AABBTreePolygonLeaf : public AABBTreeNode<Value>
{
protected :
   	typedef AABBTreePolygon<Value> PolygonType ;
public:
	int nbPols ;
	std::vector<PolygonType *> polygons ;
private:
};

template <typename Value> class AABBTreePoly ;
template <typename Value> class AABBTreeBuilder ; 

/**
 * AABBTree base class.
 */
template <typename Value>
class AABBTree
{
protected :
	typedef AABBTreeNode <Value> AABBTreeNodeType ;

private :
	/**
	 * Private contructor. We build AABBTree by Tree builder. 
	 */
	AABBTree(AABBTREE_LEAFTYPE _leafType, int _leafDepth)
	{
        leafType = _leafType;
        leafDepth = _leafDepth;
        nbNodes = 0;
        nbLeafs = 0;
//        coefVolInflater_ = 1.5f;
		root = NULL ;
	}
public :
	int nbNodes ;
	AABBTreeNodeType * root ;
	int nbLeafs ;

	AABBTREE_LEAFTYPE leafType ;
	int leafDepth ; 
	friend class AABBTreePoly <Value> ;
	friend class AABBTreeBuilder <Value> ;

private:

};


template <typename Value>
class AABBTreePolyBuilder ;

/**
 * AABBTree for polygons class.
 */
template <typename Value>
class AABBTreePoly : public AABBTree <Value>
{
protected :

	typedef AABBTreePolygon<Value>          AABBTreePolygonType ;
	typedef Vector <Value>                  VectorType ;
	typedef Vector <Value>                  PointType ;
	typedef AABBTreeNode <Value>            AABBTreeNodeType ;
	typedef AABBTreePolygonLeaf<Value>      AABBTreePolygonLeafType ;
	typedef Box<Value>                      BoxType ;
	typedef Segment<Value>                  SegmentType ;
	typedef Ray<Value>                      RayType ;

public :
	

	/**
	 * Constructor. 
	 */
	AABBTreePoly (int _leafDepth) : AABBTree<Value>( AABBTREE_LEAFTYPE_POLYGON, _leafDepth)
	{
		
	}
public:
	~ AABBTreePoly ()
	{
		_finalyze() ;
		//std::cerr<<"Destroy AABBTree"<<std::endl;
		//std::cout<<m_nbLeafs<<std::endl;
		//std::cout<<m_nbNodes<<std::endl;
	}

public :
    /**
     * Defines the result of a collision detection query with a Box.
     */
    class BoxColResult 
	{
    public:
		std::vector<const AABBTreePolygonType *> polys_;
        BoxType boxQuery_;
    };

    /**
     * Defines the result of a collision detection query with an OBB.
     */
    class OBBColResult 
	{
    public:
		std::vector<const AABBTreePolygonType *> polys_;
        OBB<Value> obbQuery_;
    };

    /**
     * Defines the result of a collision detection query with an Sphere.
     */
    class SphereColResult 
	{
    public:
		std::vector<const AABBTreePolygonType *> polys_;
        Sphere<Value> sphereQuery_;
    };

    /**
     * Defines the result of a collision detection query with an Ellipsoid.
     */
    class EllipsoidColResult 
	{
    public:
		std::vector<const AABBTreePolygonType *> polys_;
        Ellipsoid<Value> ellipsoidQuery_;
    };

    /**
     * Defines the result of a collision detection query with a segment.
     */
    class SegmentColResult 
	{
    public:
		std::vector<const AABBTreePolygonType *> polys_;
//        Vector<int> users_;
        PointType segmentPt0_;
        PointType segmentPt1_;
    };

    /**
     * Defines the result of a collision detection query with a ray.
     */
    class RayColResult 
	{
    public:
		std::vector<const AABBTreePolygonType *> polys_;
		RayType ray; 
    };


    /**
     * Defines the result of a collision detection query with a segment.
     */
    class PolygonColResult 
	{
    public:
		std::vector<const AABBTreePolygonType *> polys_;
//        Vector<int> users_;
		Polygon<Value> polygon; 
    };

	/**
	 * Define the result of AABBTree collides AABBTree. 
	 */
	class AABBTreeColResult 
	{
	public:
		/**
		 * A pair of collide polygon (triangle)
		 */
		std::vector<pair <int, int> > poly_pairs ;
		
	};
public :
	/**
	 * Test collide with box or not.
	 * @param _box the query box. 
	 * @param _cr  the collision result. 
	 * @return Boolean value. 
	 */
	bool isCollideWithBox(const BoxType &_box, BoxColResult &_cr) 
	{
		throw ("Incomplete function");
		return false; 
	}

    /**
	 * Test collide with box or not.
	 * @param _box the query box. 
	 * @param _cr  the collision result. 
	 * @return None. 
	 */
	void collideWithBox(const BoxType &_box, BoxColResult &_cr) 
	{
		this->polygonColRes_.polys_.clear();
		_collideWithBox ( (* this->root), _box);
		_cr = this->boxColRes_ ;
	}

    /**
	 * Test collide with segment
	 * @param _segment the query segment. 
	 * @param _cr  the collision result. 
	 * @return None. 
	 */
	void collideWithSegment(const SegmentType & _segment, SegmentColResult &_cr) 
	{
		this->polygonColRes_.polys_.clear();
		_collideWithSegment ( (* this->root), _segment);
		_cr = this->segmentColRes_ ;
	}

	/**
	 * Test collide with ray
	 * @param _ray the query ray.
	 * @param _cr the collision result.
	 * @return None. 
	 */
	void collideWithRay (const RayType & _ray, RayColResult &_cr)
	{
		mp_rayColRes = & _cr; 
		mp_rayColRes->polys_.clear();
		_collideWithRay ( (* this->root), _ray);
	}

	/**
	 * Test collie with polygon (Simple Polygon. Not the polygon for AABBTree. )
	 * @param _polygon the query polygon.
	 * @param _cr the collision result.
	 * @return None.
	 */
	void collideWithPolygon(const Polygon<Value> & _polygon, PolygonColResult &_cr) 
	{
		mp_polygonColRes = & _cr; 
		mp_polygonColRes->polys_.clear();
		_collideWithPolygon ( (* this->root), _polygon);
	}

	/**
	 * Test collie with another AABBTree.
	 * @param _tree the other AABBTreePoly. 
	 * @param _cr the collision result.
	 * @return None.
	 */
	void collideWithAABBTree(const AABBTreePoly<Value> & _tree, AABBTreeColResult &_cr) 
	{
		//this->polygonColRes_.polys_.clear();
		pPoints2_ = & _tree.points_; 
		mp_aabbtreeColRes = & _cr; 
		mp_aabbtreeColRes->poly_pairs.clear();
		_collideWithAABBTree ( (* this->root),*  _tree.root);
	}

private :

	bool isLeaf (const AABBTreeNodeType & _node)
	{
		//if (&_node == NULL) return false;
		return (_node.left == NULL && _node.right == NULL) ;
	}
	void _collideWithAABBTree (const AABBTreeNodeType & _node1, const AABBTreeNodeType & _node2)
	{
		//aabbtreeColRes_ ;
		//if (predicate_box_box(_node1.aabb, _node2.aabb))
		bool flag1 = isLeaf(_node1) ;
		bool flag2 = isLeaf(_node2) ;
		if (_node1.aabb.isOverlap( _node2.aabb))
		{
			if (flag1 && flag2)
			{
				const AABBTreePolygonLeafType & p1 = (AABBTreePolygonLeafType &) _node1;
				const AABBTreePolygonLeafType & p2 = (AABBTreePolygonLeafType &) _node2;
				/** This part of code is not correct when the leaf size is larger than 1. */
				Polygon<Value> pol1, pol2; 
				p1.polygons[0]->simplePolygon(pol1, points_);
				p2.polygons[0]->simplePolygon(pol2, * pPoints2_);
				if (predicate_polygon_polygon (pol1, pol2))
				{
					mp_aabbtreeColRes->poly_pairs.push_back (make_pair(p1.polygons[0]->get_face_idx(), p2.polygons[0]->get_face_idx()));
				}
			}
			else if (! flag1 && ! flag2)
			{
				if (_node1.aabb.get_volumn() > _node2.aabb.get_volumn())
				{
					_collideWithAABBTree(* _node1.left, _node2);
					_collideWithAABBTree(* _node1.right, _node2);
				}
				else
				{
					_collideWithAABBTree(_node1, * _node2.left);
					_collideWithAABBTree(_node1, * _node2.right);
				}
			}
			else if (flag1 && ! flag2)
			{
				_collideWithAABBTree(_node1, * _node2.left);
				_collideWithAABBTree(_node1, * _node2.right);
			}
			else if (! flag1 && flag2)
			{
				_collideWithAABBTree(* _node1.left, _node2);
				_collideWithAABBTree(* _node1.right, _node2);
			}
		}
	}

	void _collideWithPolygon(const AABBTreeNodeType & _node, const Polygon<Value> & _polygon)
	{
//		throw ("Incomplete function.");
        if ( predicate_polygon_box (_polygon,  _node.aabb) == true )
		{
			if (!_node.left && !_node.right) 
			{
				// reach a leaf. Only test intersection on leaves. 
				const AABBTreePolygonLeafType & pl = (AABBTreePolygonLeafType &) _node;
				int nbPols = pl.nbPols;
				for (int i = 0; i < nbPols; i++) 
				{
					AABBTreePolygonType *pol = pl.polygons[i] ;
					
					Polygon<Value> polygon ;
					pol->simplePolygon(polygon, points_);
					bool res = predicate_polygon_polygon(polygon, _polygon);
					if (res == true) 
					{
							mp_polygonColRes->polys_.push_back(pol);
					}
				}
			}
			else {
				if (_node.left)
					_collideWithPolygon((*_node.left ), _polygon);
				if (_node.right)
					_collideWithPolygon((*_node.right), _polygon);
			}
        }
	}
	void _collideWithSegment (const AABBTreeNodeType & _node, const SegmentType & _segment)
	{
        if (predicate_segment_box(_segment,  _node.aabb) == true )
		{
			if (!_node.left && !_node.right) 
			{
				// reach a leaf. Only test intersection on leaves. 
				const AABBTreePolygonLeafType & pl = (AABBTreePolygonLeafType &) _node;
				int nbPols = pl.nbPols;
				for (int i = 0; i < nbPols; i++) 
				{
					AABBTreePolygonType *pol = pl.polygons[i] ;
					
					Polygon<Value> polygon ;
					pol->simplePolygon(polygon, points_);
					bool res = predicate_polygon_segment(polygon, _segment);
					if (res == true) 
					{
							segmentColRes_.polys_.push_back(pol);
					}
				}
			}
			else {
				if (_node.left)
					_collideWithSegment((*_node.left ), _segment);
				if (_node.right)
					_collideWithSegment((*_node.right), _segment);
			}
        }
	}

	void _collideWithRay (const AABBTreeNodeType & _node, const RayType & _ray)
	{
        if (predicate_box_ray(_node.aabb, _ray) == true )
		{
			if (!_node.left && !_node.right) 
			{
				// reach a leaf. Only test intersection on leaves. 
				const AABBTreePolygonLeafType & pl = (AABBTreePolygonLeafType &) _node;
				int nbPols = pl.nbPols;
				for (int i = 0; i < nbPols; i++) 
				{
					AABBTreePolygonType *pol = pl.polygons[i] ;
					
					Polygon<Value> polygon ;
					pol->simplePolygon(polygon, points_);
					bool res = predicate_polygon_ray(polygon, _ray);
					if (res == true) 
					{
							mp_rayColRes->polys_.push_back(pol);
					}
				}
			}
			else {
				if (_node.left)
					_collideWithRay((*_node.left ), _ray);
				if (_node.right)
					_collideWithRay((*_node.right), _ray);
			}
        }
	}

	void _collideWithBox (const AABBTreeNodeType & _node, const BoxType & _box)
	{
        if (_node.aabb.isOverlap(_box) == true) 
		{
			if (!_node.left && !_node.right) 
			{
				// reach a leaf. Only test intersection on leaves. 
				const AABBTreePolygonLeafType & pl = (AABBTreePolygonLeafType &) _node;
				int nbPols = pl.nbPols;
				for (int i = 0; i < nbPols; i++) 
				{
					AABBTreePolygonType *pol = pl.polygons[i] ;
					
					Polygon<Value> polygon ;
					pol->simplePolygon(polygon, points_);
					bool res = predicate_polygon_box(polygon, _box);
					if (res == true) 
					{
						//nbColls_++;
//						if (callback_)
//							callback_(*this, *pol, user, _node.aabb, userCallback_);
//						else 
//						{						
							boxColRes_.polys_.push_back(pol);
//							boxColRes_->users_.add(user);
//						}
					}
				}
			}
			else {
				if (_node.left)
					_collideWithBox((*_node.left ), _box);
				if (_node.right)
					_collideWithBox((*_node.right), _box);
			}
        }

	}

	void _finalyze ()
	{
		for (int i = 0; i < m_nbLeafs ; ++i)
		{
			AABBTreePolygonLeafType * p = & this->leafs[i]; 
			for (int j = 0; j < p->nbPols; ++j)
			{
				delete p->polygons[j];
				p->polygons[j] = NULL ;
			}
		}
		delete [] leafs ;
		delete [] this->root; 
//		delete _node; 
//		_node = NULL ;		   		

	}
public:


private:
	BoxColResult boxColRes_ ;
	SegmentColResult segmentColRes_ ;

	RayColResult * mp_rayColRes ;
	PolygonColResult * mp_polygonColRes ;
	AABBTreeColResult * mp_aabbtreeColRes ;
	
public :
	AABBTreePolygonLeafType * leafs;
	std::vector<PointType> points_; 
	std::vector<PointType> const * pPoints2_ ;
	int m_nbNodes ; 
	int m_nbLeafs ;
};

/**
 * Auxiliary class for build AABBTree. 
 */
template <typename Value>
class AABBTreePolyBuilder 
{
protected :
	typedef Value ValueType; 
	typedef Box<Value> BoxType ;
	typedef AABBTreePolygon<Value> PolygonType; 
	typedef AABBTreePolygon<Value> AABBTreePolygonType;
	typedef AABBTreePoly <Value> AABBTreePolyType;
	typedef AABBTreeNode <Value> AABBTreeNodeType ;
	typedef AABBTreePolygonLeaf<Value> AABBTreePolygonLeafType ;
	typedef Vector<Value> PointType ;
	typedef Vector<Value> VectorType ;
private:
   /**
	* For create the tree represented by an array. 
	*/
	class WorkingItem 
	{
	public:
	WorkingItem() : left(-1), right(-1) {}
	public:
		BoxType aabb ;
		int left, right ; // if it is a left, both are -1s. 
		std::vector<PolygonType * > polygons; 
	};

	std::vector<WorkingItem *> workingItems ;
	AABBTreePolyType * tree ; 
public :
	AABBTreePolyBuilder() {}
	~AABBTreePolyBuilder () {}

	AABBTreePolyType * build_from_mesh (Mesh<MeshPrimitive<Value> > & mesh, int _leafDepth)
	{
		//Prepare the vertices array ;
		std::vector <PointType> vertices ;
		std::vector <AABBTreePolygonType *> tris ;
		for(unsigned i = 0; i < mesh.vertices.size(); ++i)//reading vertices
		{
			//Add vertex positions
			Vertex<Value> * v = mesh.vertices[i]; 
			vertices.push_back(v->m_coord);
		}

		tris.resize(mesh.faces.size());
		for(unsigned i = 0; i < mesh.faces.size(); ++i)
		{
			//we must build each face before add it to the list
			AABBTreePolygonType * p = new AABBTreePolygonType;

			std::vector<int> v_set ;
			mesh.faces[i]->Face2VertexIdx(v_set);

			p->set_nb_vertices(v_set.size()) ;
			for (unsigned j = 0; j< v_set.size(); ++j)
			{
				p->setIndex (j, v_set[j]);
			}
		
			p->set_normal(mesh.faces[i]->normal());//adding normals (previously readed)
			p->set_face_idx (mesh.faces[i]->m_idx);
			p->set_face (mesh.faces[i]);
			//adding the polygon to the polygon list
			tris[i] = p;
		}
		return buildFromPolygons (tris, vertices, _leafDepth) ;		
	}

	AABBTreePolyType * buildFromPolygons (const std::vector<PolygonType * > & polygons, const std::vector<PointType> & _points, int leafDepth )
	{
		int nbPolygons = polygons.size() ; 
		//int nbPoints = points.size(); 
		tree = new AABBTreePolyType (leafDepth);
		/**
		 * Copy all the points to the tree's internal. 
		 */
		tree->points_ = _points; 

		WorkingItem * item = new WorkingItem(); 
		for (int i = 0; i < nbPolygons; ++i)
		{
			item->polygons.push_back(polygons[i]); 
		}
		item->left = -1; 
		item ->right = -1;
		workingItems.push_back(item); 
		int off = 0;
		while (true)
		{
			WorkingItem & it = * workingItems[off]; 
			workOnItems (it, _points, leafDepth); 
			off++; 
			int size = workingItems.size(); 
			if (off == size) break; 
		}

		buildFromWorkingItems (); 
		// clear the auxiliary memory.
		for (unsigned i = 0; i < workingItems.size(); ++i )
		{
			delete (workingItems[i]); 
		}

		workingItems.clear();

		return tree ;
	}
	void workOnItems (WorkingItem & item ,  const std::vector<PointType> & points, int leafDepth )
	{
		int size = item.polygons.size();
		if (size <= leafDepth)
		{
			item.left = -1; item.right = -1; 
			return ; 
		}
		BoxType boundbox ;
		int axis ;
		this->findBoundingBox(item.polygons, points, boundbox, axis);

		ValueType middle = calculateAvgPoint(item.polygons, points, axis); 
		
		VectorType bboxMin = boundbox.getMin() ;
		VectorType bboxMax = boundbox.getMax() ;

		item.aabb.center = boundbox.center ;
		item.aabb.extent = boundbox.extent ;


		WorkingItem * left = NULL ;
		WorkingItem * right = NULL ;

//		std::vector<Polygon * > left_polygons ;
//		std::vector<Polygon * > right_polygons;

		for (unsigned i = 0; i < item.polygons.size(); ++i)
		{
			PolygonType * pol = item.polygons[i];
			ValueType min, max ;
			int rValue = classifyPol( * pol, points, axis, middle, min, max ); 
			if (0 == rValue) // rValue = 0 : assign to left
			{
				if (NULL == left)
				{
					left = new WorkingItem ;
					BoxType & box = left->aabb ;
					if (axis == 0) box.setFromPoints(bboxMin, VectorType((bboxMin[0] + bboxMax[0]) / 2.0f, bboxMax[1], bboxMax[2]));
					else if (axis == 1) box.setFromPoints(bboxMin, VectorType (bboxMax[0], (bboxMin[1] + bboxMax[1]) / 2.0f, bboxMax[2]));
                    else if (axis == 2) box.setFromPoints(bboxMin, VectorType (bboxMax[0], bboxMax[1], (bboxMin[2] + bboxMax[2]) / 2.0f));
				}
				BoxType & box = left->aabb ;
				if (axis == 0 && max > (box.getCenter()[0] + box.getExtent()[0]) )
					box.setFromPoints(bboxMin, VectorType(max, bboxMax[1], bboxMax[2]));
				else if (axis == 1 && max > (box.getCenter()[1] + box.getExtent()[1]) )
					box.setFromPoints(bboxMin, VectorType(bboxMax[0], max, bboxMax[2]));
				else if (axis == 2 && max > (box.getCenter()[2] + box.getExtent()[2]) )
					box.setFromPoints(bboxMin, VectorType(bboxMax[0], bboxMax[1], max));
				left->polygons.push_back(pol);
			}
			else  // rValue = 1 : assign to right
			{
				if (NULL == right)
				{
					right = new WorkingItem ;
					BoxType & box = right->aabb;
					if (axis == 0) box.setFromPoints(VectorType((bboxMin[0] + bboxMax[0]) / 2.0f, bboxMin[1], bboxMin[2]), bboxMax);
					else if (axis == 1) box.setFromPoints(VectorType(bboxMin[0], (bboxMin[1] + bboxMax[1]) / 2.0f, bboxMin[2]), bboxMax);
					else if (axis == 2) box.setFromPoints(VectorType(bboxMin[0], bboxMin[1], (bboxMin[2] + bboxMax[2]) / 2.0f), bboxMax);
				}
				BoxType & box = right->aabb;
				if (axis == 0 && min < (box.getCenter()[0] - box.getExtent()[0]) )
					box.setFromPoints(VectorType(min, bboxMin[1], bboxMin[2]), bboxMax);
				else if (axis == 1 && min < (box.getCenter()[1] - box.getExtent()[1]) )
					box.setFromPoints(VectorType(bboxMin[0], min, bboxMin[2]), bboxMax);
				else if (axis == 2 && min < (box.getCenter()[2] - box.getExtent()[2]) )
					box.setFromPoints(VectorType(bboxMin[0], bboxMin[1], min), bboxMax);
				right->polygons.push_back(pol);

			}
		}

		if ((left && !right) || (!left && right)) 
		{
			if (left && !right)
			{
				right = new WorkingItem ;
				right->aabb = left->aabb; 
				int nbPols = left->polygons.size();
				int le = nbPols / 2;
				int start = nbPols - le ;
				for (int i = start ;i < nbPols; ++i)
				{
					PolygonType * p = left->polygons[i];
					right->polygons.push_back(p);
				}
				left->polygons.resize(start);
			}
			else if (!left && right)
			{
				left = new WorkingItem ;
				left->aabb = right->aabb ;
				int nbPols = right->polygons.size();
				int le = nbPols / 2;
				int start = nbPols - le ;
				for (int i = start ;i < nbPols; ++i)
				{
					PolygonType * p = right->polygons[i];
					left->polygons.push_back(p);
				
				}
				right->polygons.resize(start);
			}
		}	
		if (left)
		{
			item.left = workingItems.size(); 
			workingItems.push_back(left); 
		}
		else item.left = -1;
		if (right)
		{
			item.right = workingItems.size();
			workingItems.push_back(right);
		}
		else
			item.right = -1 ;
	}
	void buildFromWorkingItems ()
	{
		int * ln = new int[workingItems.size()];
		int nbNodes = 0; 
		int nbLeafs = 0;
		for (unsigned i = 0; i < workingItems.size(); ++i)
		{
			WorkingItem & it = * workingItems[i];
			if (it.left == -1 && it.right == -1)
				ln[i] = nbLeafs++ ;
			else
				ln[i] = nbNodes++ ;
		}
		
		tree->root = new AABBTreeNodeType[nbNodes];
		tree->leafs = new AABBTreePolygonLeafType [nbLeafs];
		tree->m_nbLeafs = nbLeafs; 
		tree->m_nbNodes = nbNodes ;


		for (unsigned i = 0; i < workingItems.size(); ++i)
		{
			WorkingItem & it = * workingItems [i] ;
			if (it.left == -1 && it.right == -1)
			{
				int indexLeaf = ln [i] ;
				AABBTreePolygonLeafType * p1 = (AABBTreePolygonLeafType *) tree->leafs ;
				p1 += indexLeaf ;
				p1->aabb = it.aabb ;
				p1->left = NULL ;
				p1->right = NULL ;
				int nbPols = it.polygons.size();
				p1->nbPols = nbPols ;
				p1->polygons.resize(nbPols);
				for (int j = 0; j < nbPols; ++j)
				{
					p1->polygons[j] = it.polygons[j] ;
					it.polygons[j] = NULL ;
				}
			}
			else
			{
				int nodeIndex = ln[i] ;
				AABBTreeNodeType * pn = (tree->root)+ nodeIndex ;
				pn->aabb = it.aabb ;
				if (it.left != -1)
				{
					WorkingItem * item = workingItems [it.left] ;
					if (item->left == -1 && item->right == -1)
					{
						AABBTreePolygonLeafType * pl = (AABBTreePolygonLeafType * ) tree->leafs; 
						pn -> left = pl + ln[it.left];
					}
					else
					{
						pn->left = tree->root + ln [it.left];
					}
				}
				else
				{
					pn->left = NULL ;
				}
				if (it.right != -1)
				{
					WorkingItem * item = workingItems[it.right];
					if (item->left == -1 && item->right == -1)
					{
						AABBTreePolygonLeafType * pl = (AABBTreePolygonLeafType *) tree->leafs ;
						pn->right = pl + ln[it.right];
					}
					else
					{
						pn->right = tree->root + ln[it.right];
					}
				}
				else pn->right = NULL ;

			}
		}
		delete [] ln; 

		
	}
protected:
	void findBoundingBox (const std::vector<PolygonType *> & polygons, 
						  const std::vector<PointType> & points, 
						  BoxType & box, int & axis)
	{
		VectorType min, max;
		min = VectorType(FLOAT_INFINITY, FLOAT_INFINITY, FLOAT_INFINITY);
		max = min * ValueType(-1.0f); 
		for (unsigned i = 0; i < polygons.size(); ++i)
		{
			PolygonType & pol = * polygons[i] ;
			unsigned  nbVerts = pol.get_nb_vertices(); 
			for (unsigned j = 0; j < nbVerts ; ++j)
			{
				int idx = pol.getIndex(j); 
				const VectorType & pt = points.at(idx) ;
				if (pt[0] < min[0]) min[0] = pt[0];
				if (pt[1] < min[1]) min[1] = pt[1];
				if (pt[2] < min[2]) min[2] = pt[2];
				if (pt[0] > max[0]) max[0] = pt[0];
				if (pt[1] > max[1]) max[1] = pt[1];
				if (pt[2] > max[2]) max[2] = pt[2];
			}
		}
		box = BoxType ( (min + max) * ValueType(0.5), (max - min) * ValueType(0.5)); 

		//Find the split axis 
		VectorType ext = box.getExtent(); 
		if (ext[0] > ext[1] && ext[0] > ext[2])
			axis = 0;
        else {
			if (ext[1] > ext[2])
				axis = 1;
			else
				axis = 2;
        }

	}

	int classifyPol(const PolygonType &_pol, const std::vector<VectorType> & points_, int _axis, ValueType _middle, ValueType &_min, ValueType &_max)
	{
        int i;

        takeMinMax(_pol, points_, _axis, _min, _max);
        int nbVerts = _pol.get_nb_vertices();
        ValueType m = 0.0f;
        for (i = 0; i < nbVerts; i++) {
			int index = _pol.getIndex(i);
			VectorType pt = points_[index];
			if (_axis == 0)
				m += pt[0];
			else if (_axis == 1)
				m += pt[1];
			else if (_axis == 2)
				m += pt[2];
        }
        m /= nbVerts;
        if (m < _middle)
			return 0;
        return 1;
	}
	void takeMinMax(const PolygonType &_pol, const std::vector<VectorType> & points_, int _axis, ValueType &_min, ValueType &_max)
	{
        int i;
        int nbVerts = _pol.get_nb_vertices();
        _min = FLOAT_INFINITY;
        _max = - FLOAT_INFINITY;
        for (i = 0; i < nbVerts; i++) {
			int index = _pol.getIndex(i);
			VectorType pt = points_[index];

			if (_axis == 0 && pt[0] < _min)
				_min = pt[0];
			else if (_axis == 1 && pt[1] < _min)
				_min = pt[1];
			else if (_axis == 2 && pt[2] < _min)
				_min = pt[2];

			if (_axis == 0 && pt[0] > _max)
				_max = pt[0];
			else if (_axis == 1 && pt[1] > _max)
				_max = pt[1];
			else if (_axis == 2 && pt[2] > _max)
				_max = pt[2];
        }
	}

	ValueType calculateAvgPoint(const std::vector<PolygonType *> _item,  const std::vector<PointType> & points_, int _axis)
	{
        int i, j;

        int nb = 0;
        int nbPols = _item.size();

        ValueType m = 0;
        if (_axis == 0) {
			for (i = 0; i < nbPols; i++) {
				PolygonType &pol = *_item[i];
				int nbVerts = pol.get_nb_vertices();
				nb += nbVerts;
				for (j = 0; j < nbVerts; j++) {
					int index = pol.getIndex(j);
					m += points_[index][0];
				}
                        
			}
        }
        else if (_axis == 1) {
			for (i = 0; i < nbPols; i++) {
				PolygonType &pol = *_item[i];
				int nbVerts = pol.get_nb_vertices();
				nb += nbVerts;
				for (j = 0; j < nbVerts; j++) {
					int index = pol.getIndex(j);
					m += points_[index][1];
				}
                        
			}
        }
        else if (_axis == 2) {
			for (i = 0; i < nbPols; i++) {
				PolygonType &pol = *_item[i];
				int nbVerts = pol.get_nb_vertices();
				nb += nbVerts;
				for (j = 0; j < nbVerts; j++) {
					int index = pol.getIndex(j);
					m += points_[index][2];
				}
                        
			}
        }
        return m / float(nb);
	}

};



#endif
